﻿#include "aacore.h"
#include <QPainter>
#include <utils.h>
#include "namedefinition.h"
#include "../AAHead/aahead.h"
#define PI  3.14159265

SILICOOL_CREATE_LOGGING_CATEGORY(aaCoreTimeLog, "aaCoreTimeLog")

typedef enum {
    AA_ZSCAN_NORMAL,
    AA_DFOV_MODE,
    AA_STATIONARY_SCAN_MODE,
    AA_XSCAN_MODE //Special AA scan mode for KunLunShan project
} ZSCAN_MODE;

QString getCurrentTimeString()
{
    QDateTime local = QDateTime::currentDateTime();
    QString timeString = local.toString("HH-mm-ss-zzz");
    return timeString;
}

QString getCurrentDateString()
{
    QDateTime local = QDateTime::currentDateTime();
    QString timeString = local.toString("yyyy-MM-dd");
    return timeString;
}

QString getDir(QString dir)
{
    QString dir_name;
    if (!QDir(AALog::AABaseLogDir).exists()) { QDir().mkdir(AALog::AABaseLogDir); }
    if (!QDir(QString(AALog::AABaseLogDir).append(dir)).exists()) {
         QDir().mkdir(QString(AALog::AABaseLogDir).append(dir));
    }
    if (!QDir(QString(AALog::AABaseLogDir).append(dir).append(getCurrentDateString()).append("//")).exists()) {
         QDir().mkdir(QString(AALog::AABaseLogDir).append(dir).append(getCurrentDateString()).append("//"));
    }
    return QString(AALog::AABaseLogDir).append(dir).append(getCurrentDateString()).append("//");
}


vector<double> fitCurve(const vector<double> & x, const vector<double> & y, int order, double & localMaxX, double & localMaxY, vector<double> & y_output) {
    size_t n = x.size();

    double minX = 999999;
    double maxX = -999999;

    for (size_t i = 0; i < n; i++) {
        minX = std::min(minX, x[i]);
        maxX = std::max(maxX, x[i]);
    }

    Eigen::MatrixXd X(n, order + 1);
    for (size_t i = 0; i < n; ++i) {
        double tmp = 1;
        for (int j = 0; j <= order; ++j) {
            X(i, j) = tmp;
            tmp *= x[i];
        }
    }
    Eigen::MatrixXd Y(n, 1);
    for (size_t i = 0; i < n; ++i) {
        Y(i, 0) = y[i];
    }
    Eigen::MatrixXd Xt(order + 1, n);
    Xt = X.transpose();

    Eigen::MatrixXd XtX(order + 1, order + 1);
    XtX = Xt * X;
    Eigen::MatrixXd XtX_inverse(order + 1, order + 1);
    XtX_inverse = XtX.inverse();

    Eigen::MatrixXd A(order + 1, 1);
    A = XtX_inverse * Xt * Y;

    Eigen::MatrixXd B(order + 1, 1);
    B = X * A;

    Eigen::MatrixXd Ans(n, 1);
    Ans = X * A;

    vector<double> ans;
    for (int i = 0; i <= order; ++i) {
        ans.push_back(A(i, 0));
    }

    double delta = (maxX - minX) / 300;
    localMaxY = -999999;
    localMaxX = minX;
    for (double z = minX; z <= maxX; z += delta)
    {
        double tmp = 1; double ey = 0;
        for (int i = 0; i <= order; ++i) {
            ey += A(i,0)*tmp;
            tmp *= z;
        }
        if (ey > localMaxY) {
            localMaxX = z;
            localMaxY = ey;
        }
    }
    for (size_t j = 0; j < x.size(); j++){
        double tmp = 1; double ey = 0;
        for (int i = 0; i <= order; ++i){
            ey += A(i,0)*tmp;
            tmp *= x[j];
        }
        y_output.push_back(ey);
    }
    return ans;
}

AACore::AACore() : Worker(WorkerName::AACore)
{
    aaCoreConfig = new AACoreConfig(this);
    aaCoreConfigFile = new ConfigFile("AACoreConfig", aaCoreConfig, QString("%1/AACoreConfig.json").arg(getWorkerConfigDir()));
    aaCoreConfigFile->populate();
    //Init Image Provider
    liveImageProvider = new ImageProvider();
    mtfImageProvider = new ImageProvider();
    ocImageProvider = new ImageProvider();
    aaDebugImageProvider = new ImageProvider();
    //Init Sfrwoker
    sfrWorkerController = new SfrWorkerController(this);
    xfGrabber = new XFGraber();

    if (!QDir(AALog::AABaseLogDir).exists()) { QDir().mkdir(AALog::AABaseLogDir); }
    if (!QDir(AALog::GrabberLogDir).exists()) { QDir().mkdir(AALog::GrabberLogDir); }
}

void AACore::initEventHandle()
{
    sUTXYZ = MotionElementContainer::getIns()->getItem<XYZModule>(XYZModuleName::SUTXYZ);
    aaThetaXYZ = MotionElementContainer::getIns()->getItem<XYZModule>(XYZModuleName::AaHeadTheta);
}


AACore::~AACore()
{
}


void AACore::startEventHandle(const WorkerEvent &event)
{
    Q_UNUSED(event)
    isAaHeadStopped = false;
    //pushEvent(WorkerEvent("startRun"));
}

void AACore::allWorkerStopped()
{
    isAaHeadStopped = true;
}

void AACore::purgeEventHandle()
{
    purgeEventHandled();
}

void AACore::resetEventHandle()
{
    isAaHeadStopped = false;
}

void AACore::homeWorker(bool moveToReadyPosAfterHome)
{
    //.all axis home


    if(moveToReadyPosAfterHome)
    {
        //.moveToStanbyPos();
    }
}

void AACore::setAAHead(AaHead *aaHead)
{
    this->aaHead = aaHead;
}

void AACore::beginThread() {
    if (m_pDisplayThread == nullptr) {
        m_pDisplayThread = new QThread;
        m_pObjdisplayThread = new DisplayThread(0);
        m_pObjdisplayThread->setXFGrabber(this->xfGrabber);
        connect(m_pDisplayThread,SIGNAL(finished()),m_pDisplayThread,SLOT(deleteLater()));
        connect(m_pDisplayThread,SIGNAL(started()),m_pObjdisplayThread,SLOT(startThread()));
        connect(m_pDisplayThread,SIGNAL(finished()),this,SLOT(displayThreadExit()));
        connect(m_pObjdisplayThread, SIGNAL(displayLiveImage(QImage)), this, SLOT(drawBmp24(QImage)));
        m_pObjdisplayThread->moveToThread(m_pDisplayThread);
    }
    if (!m_pDisplayThread->isRunning()) {
        m_pDisplayThread->start();
    }
}

void AACore::endThread() {
    if(m_pDisplayThread != nullptr && m_pDisplayThread->isRunning())
    {
        m_pObjdisplayThread->closeThread();
        m_pDisplayThread->quit();
        m_pDisplayThread->wait();
    }
}

void AACore::drawBmp24(QImage image) {
    setLiveImageDisplayStatus(true);
    liveImageProvider->setImage(image);
    emit displayAAImageInUI(AAImageDefinition::LiveImage);
}

void AACore::displayThreadExit() {
    qCInfo(logCate()) << "Display Thread Exit ";
    setLiveImageDisplayStatus(false);
}

void AACore::test()
{
    try {
        qCInfo(logCate()) << tr("test");
        throw SilicoolException("test");
    } catch (SilicoolException &se)
    {
        auto res = UIOperation::getIns()->yesNoConfirm(tr("%1, would you like to retry?").arg(se.what()));
        if(res)
        {
            qCInfo(logCate()) << "OK";
        }
        else {
            qCInfo(logCate()) << "Abort";
        }
        qCCritical(logCate()) << se.what();
    }
}

void AACore::loadFlowchart(QString json)
{
    qCDebug(logCate()) << tr("Update flowchart from webview");
    currentFlowchartDocument = QJsonDocument::fromJson(json.toUtf8());
}

bool AACore::performTest(QString testItemName, QJsonValue properties)
{
    QString testName = properties["title"].toString();
    QJsonValue params = properties["params"];
    int retry_count = params["retry"].toInt(0);
    QJsonValue delay_in_ms_qjv = params["delay_in_ms"];
    unsigned int delay_in_ms = delay_in_ms_qjv.toInt(0);

    qCInfo(logCate()) << tr("Running Test: ") << testItemName;
    SCTimer t(QString("Perform ").append(testItemName), aaCoreTimeLog());

    if (testItemName.contains(AA_PIECE_START)) { qCInfo(logCate()) << tr("Performing Start testitem"); }
    else if (testItemName.contains(AA_PIECE_OC)) {
        bool ret = performOC(params);
        if (!ret) {
            qCWarning(logCate()) << tr("Perform OC fail! Set Ng Lens");
            setNgLens();
        }
        return ret;
    }
    else if (testItemName.contains(AA_PIECE_MTF)) {
        bool ret = performMTF(params);
        if (!ret) {
            qCWarning(logCate()) << tr("Perform MTF fail! Set Ng Lens");
            setNgLens();
        }
        return ret;
    }
    else if (testItemName.contains(AA_PIECE_DELAY)) {
        return performDelay(params);
    }
    else if (testItemName.contains(AA_PIECE_AA)) {
        bool ret = performAA(params);
        if (!ret) {
            qCWarning(logCate()) << tr("Perform AA fail! Set Ng Lens");
            setNgLens();
        }
        return ret;
    }
    else if (testItemName.contains(AA_PIECE_INIT_CAMERA)) {
        bool ret = performInitCamera(params);
        if (!ret) {
            qCWarning(logCate()) << tr("Perform Init Camera fail! Set Ng Sensor");
            setNgSensor();
        }
        return ret;
    }
    else if (testItemName.contains(AA_PIECE_DISPENSE)) {
        bool ret = performDispense(params);
        if (!ret) {
            qCWarning(logCate()) << tr("Perform Init Camera fail! Set Ng Sensor");
            setNgProduct();
        }
        return ret;
    }
    else if (testItemName.contains(AA_PIECE_Z_OFFSET)) {
       bool ret = performZOffset(params);
       return ret;
    }
    else if (testItemName.contains(AA_PIECE_UV)) {
        bool ret = performUV(params);
        return ret;
    }

    return false;
}

void AACore::performAATest()
{
    double start = aaCoreConfig->aaAnalysisStartPos();
    double step_size = aaCoreConfig->aaAnalysisStepSize()/1000;
    unsigned int zSleepInMs = aaCoreConfig->aaAnalysisDelayInMs();
    int imageCount = aaCoreConfig->aaAnalysisCount();
    int sfrCount = 0;
    int resize_factor = aaCoreConfig->Oversampling() + 1;
    qCInfo(logCate()) << "step_size: " << step_size << " start pos: " << start;

    for (int i = 0; i < imageCount; i++)
    {
        qCInfo(logCate()) << "Z Scan : " << i;
        this->sUTXYZ->zAxis()->absMove(start + step_size*i);
        QThread::msleep(zSleepInMs);
        cv::Mat input_img = xfGrabber->grabImageCV();

//      QString filename = "C:\\Users\\emil\\Desktop\\AAProject\\newSingleHead\\U_M_2020-09-07\\U_0.3\\zscan_" + QString::number(i) + ".jpg";
//      cv::Mat input_img = cv::imread(filename.toStdString());

        if (input_img.empty()) {
            qCWarning(logCate()) << tr("Perform AA tilt calibration fail due to cannot grab image");
            return;
        }
        cv::Mat img;
        cv::Size size(input_img.cols/resize_factor, input_img.rows/resize_factor);
        cv::resize(input_img, img, size);
        emit sfrWorkerController->calculate(i, start+i*step_size, img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea(), aaCoreConfig->SampleFrequency()+1, aaCoreConfig->AASelectedLayer()+1);
        img.release();
        sfrCount++;
        qCInfo(logCate()) << "Z Scan finish : " << i;
    }

    int timeout=2000;
    while(this->clustered_sfr_map.size() != sfrCount && timeout > 0) {
        Sleep(10);
        timeout--;
    }
    if (timeout <= 0) {
        qCCritical(logCate())<< tr("Error in performing AA Test Calibration. Timeout reached: %1").arg(timeout);
        return;
    }
    QVariantMap aa_result = sfrFitCurve_Advance(resize_factor, start);
    last_aa_result = aa_result;
    clustered_sfr_map.clear();
}

double slope(const vector<double>& x, const vector<double>& y){
    if(x.size() != y.size()){
        return -99;
    }
    size_t n = x.size();
    double avgX = accumulate(x.begin(), x.end(), 0.0) / n;
    double avgY = accumulate(y.begin(), y.end(), 0.0) / n;
    double numerator = 0.0;
    double denominator = 0.0;
    for(size_t i=0; i<n; ++i){
        numerator += (x[i] - avgX) * (y[i] - avgY);
        denominator += (x[i] - avgX) * (x[i] - avgX);
    }
    if(denominator == 0.0){
        return -99;
    }
    return numerator / denominator;
}

void AACore::performFOVCalibration()
{
    double start = aaCoreConfig->aaAnalysisStartPos();
    double step_size = aaCoreConfig->aaAnalysisStepSize()/1000;
    unsigned int zSleepInMs = aaCoreConfig->aaAnalysisDelayInMs();
    int imageCount = aaCoreConfig->aaAnalysisCount();
    int sfrCount = 0;
    int resize_factor = aaCoreConfig->Oversampling() + 1;
    qCInfo(logCate()) << "step_size: " << step_size << " start pos: " << start;

    vector<double> fov_v, z_v;
    for (int i = 0; i < imageCount; i++)
    {
        qCInfo(logCate()) << "Z Scan : " << i;
        this->sUTXYZ->zAxis()->absMove(start + step_size*i);
        //QString filename = "C:\\Users\\emil\\Desktop\\AAProject\\newSingleHead\\U_M_2020-09-07\\U_0.3\\zscan_" + QString::number(i) + ".jpg";
        //cv::Mat input_img = cv::imread(filename.toStdString());
        QThread::msleep(zSleepInMs);
        cv::Mat input_img = xfGrabber->grabImageCV();
        double dfov = calculateDFOV(input_img);
        fov_v.push_back(dfov);
        z_v.push_back(start + step_size*i);
    }
    double fov_slope = slope(z_v, fov_v);

    performAATest();

    double x_tilt_offset = last_aa_result["yTilt"].toDouble()/aaCoreConfig->aaCalibrationYTilt();
    double y_tilt_offset = last_aa_result["xTilt"].toDouble()/aaCoreConfig->aaCalibrationXTilt();
    this->aaThetaXYZ->xAxis()->relMove(x_tilt_offset);
    this->aaThetaXYZ->yAxis()->relMove(y_tilt_offset);
    this->sUTXYZ->zAxis()->absMove(last_aa_result["zPeak"].toDouble());
    QThread::msleep(zSleepInMs);

    cv::Mat input_img = xfGrabber->grabImageCV();
    double dfov = calculateDFOV(input_img);

    QString resultString = "";
    resultString.append("Update DFOV Calibration Result? DFOV Slope: ").append(QString::number(fov_slope))
                .append(" AA FOV: ").append(QString::number(dfov));
    if (UIOperation::getIns()->yesNoConfirm(resultString))
    {
        aaCoreConfig->setEstimatedAAFOV(dfov);
        aaCoreConfig->setFOVSlope(fov_slope);
    }
}

void AACore::performAATiltCalibration()
{
    QString informationMessage;
    informationMessage.append(tr("Perform AA Tilt Calibration? "));
    if (!UIOperation::getIns()->okCancelConfirm(informationMessage))
    {
        return;
    }

    double init_x_tilt_pos = this->aaThetaXYZ->xAxis()->getFeedbackPos();
    double init_y_tilt_pos = this->aaThetaXYZ->yAxis()->getFeedbackPos();

    uint dataCount = 5;

    double xtiltResult_1[10][4];
    double xtiltResult_2[10][4];

    double ytiltResult_1[10][4];
    double ytiltResult_2[10][4];

    // + X Tilt Test
    for (int j = 0; j < dataCount; j++)
    {
        this->aaThetaXYZ->xAxis()->relMove(0.1);
        performAATest();
        double xTilt = last_aa_result["xTilt"].toDouble();
        double yTilt = last_aa_result["yTilt"].toDouble();
        qCInfo(logCate()) << "xTilt: " << xTilt << " yTilt: " << yTilt;
        xtiltResult_1[j][0] = xTilt;
        xtiltResult_1[j][1] = yTilt;
        xtiltResult_1[j][2] = this->aaThetaXYZ->xAxis()->getFeedbackPos();
        xtiltResult_1[j][3] = this->aaThetaXYZ->yAxis()->getFeedbackPos();
    }
    this->aaThetaXYZ->xAxis()->absMove(init_x_tilt_pos);

    // - X Tilt Test
    for (int j = 0; j < dataCount; j++)
    {
        this->aaThetaXYZ->xAxis()->relMove(-0.1);
        performAATest();
        double xTilt = last_aa_result["xTilt"].toDouble();
        double yTilt = last_aa_result["yTilt"].toDouble();
        qCInfo(logCate()) << "xTilt: " << xTilt << " yTilt: " << yTilt;
        xtiltResult_2[j][0] = xTilt;
        xtiltResult_2[j][1] = yTilt;
        xtiltResult_2[j][2] = this->aaThetaXYZ->xAxis()->getFeedbackPos();
        xtiltResult_2[j][3] = this->aaThetaXYZ->yAxis()->getFeedbackPos();
    }
    this->aaThetaXYZ->xAxis()->absMove(init_x_tilt_pos);

    // + Y Tilt Test
    for (int j = 0; j < dataCount; j++)
    {
        this->aaThetaXYZ->yAxis()->relMove(0.1);
        performAATest();
        double xTilt = last_aa_result["xTilt"].toDouble();
        double yTilt = last_aa_result["yTilt"].toDouble();
        qCInfo(logCate()) << "xTilt: " << xTilt << " yTilt: " << yTilt;
        ytiltResult_1[j][0] = xTilt;
        ytiltResult_1[j][1] = yTilt;
        ytiltResult_1[j][2] = this->aaThetaXYZ->xAxis()->getFeedbackPos();
        ytiltResult_1[j][3] = this->aaThetaXYZ->yAxis()->getFeedbackPos();
    }
    this->aaThetaXYZ->yAxis()->absMove(init_y_tilt_pos);

    // - Y Tilt Test
    for (int j = 0; j < dataCount; j++)
    {
        this->aaThetaXYZ->yAxis()->relMove(-0.1);
        performAATest();
        double xTilt = last_aa_result["xTilt"].toDouble();
        double yTilt = last_aa_result["yTilt"].toDouble();
        qCInfo(logCate()) << "xTilt: " << xTilt << " yTilt: " << yTilt;
        ytiltResult_2[j][0] = xTilt;
        ytiltResult_2[j][1] = yTilt;
        ytiltResult_2[j][2] = this->aaThetaXYZ->xAxis()->getFeedbackPos();
        ytiltResult_2[j][3] = this->aaThetaXYZ->yAxis()->getFeedbackPos();
    }
    this->aaThetaXYZ->yAxis()->absMove(init_y_tilt_pos);

    bool needReverse = false;
    double xTiltCalibrationResult = 0;
    double yTiltCalibrationResult = 0;
    {
        vector<double> xTiltEnc_v, xTilt_v, yTilt_v;
        for (int i = 0; i < dataCount; i++){
            xTiltEnc_v.push_back(xtiltResult_1[i][2]);
            xTiltEnc_v.push_back(xtiltResult_2[i][2]);

            xTilt_v.push_back(xtiltResult_1[i][0]);
            xTilt_v.push_back(xtiltResult_2[i][0]);

            yTilt_v.push_back(xtiltResult_1[i][1]);
            yTilt_v.push_back(xtiltResult_2[i][1]);
        }
        double slope1 = slope(xTiltEnc_v, xTilt_v);
        double slope2 = slope(xTiltEnc_v, yTilt_v);
        if (fabs(slope1) < fabs(slope2)) {
            qCInfo(logCate()) << tr("x y tilt need reverse");
            needReverse = true;
            xTiltCalibrationResult = 1/slope2;
        } else {
            xTiltCalibrationResult = 1/slope1;
        }
    }

    {
        vector<double> yTiltEnc_v, xTilt_v, yTilt_v;
        for (int i = 0; i < dataCount; i++){
            yTiltEnc_v.push_back(ytiltResult_1[i][3]);
            yTiltEnc_v.push_back(ytiltResult_2[i][3]);

            xTilt_v.push_back(ytiltResult_1[i][0]);
            xTilt_v.push_back(ytiltResult_2[i][0]);

            yTilt_v.push_back(ytiltResult_1[i][1]);
            yTilt_v.push_back(ytiltResult_2[i][1]);

        }
        double slope1 = slope(yTiltEnc_v, xTilt_v);
        double slope2 = slope(yTiltEnc_v, yTilt_v);
        if (fabs(slope1) < fabs(slope2)) {
            yTiltCalibrationResult = 1/slope2;
        } else {
            yTiltCalibrationResult = 1/slope1;
        }
    }
    QString resultString = "";
    resultString.append("Update Tilt Calibration Result? X Tilt Ratio: ").append(QString::number(xTiltCalibrationResult))
                .append("Y Tilt Ratio: ").append(QString::number(yTiltCalibrationResult))
                .append( " Need Reverse: ").append(QString::number(needReverse));
    if (UIOperation::getIns()->yesNoConfirm(resultString))
    {
        aaCoreConfig->setAACalibrationTiltNeedReverse(needReverse);
        aaCoreConfig->setAACalibrationXTilt(xTiltCalibrationResult);
        aaCoreConfig->setAACalibrationYTilt(yTiltCalibrationResult);
    }

//    for (int i = 0; i < dataCount; i++){
//        qCInfo(logCate()) << "xtiltResult_2 xTilt: " << xtiltResult_2[i][0] << " yTilt: " << xtiltResult_2[i][1]<< " xTiltEnc: " << xtiltResult_2[i][2] << " yTiltEnc: " << xtiltResult_2[i][3];
//    }
//    for (int i = 0; i < dataCount; i++){
//        qCInfo(logCate()) << "xtiltResult_2 xTilt: " << xtiltResult_2[i][0] << " yTilt: " << xtiltResult_2[i][1]<< " xTiltEnc: " << xtiltResult_2[i][2] << " yTiltEnc: " << xtiltResult_2[i][3];
//    }
//    for (int i = 0; i < dataCount; i++){
//        qCInfo(logCate()) << "ytiltResult_1 xTilt: " << ytiltResult_1[i][0] << " yTilt: " << ytiltResult_1[i][1] << " xTiltEnc: " << ytiltResult_1[i][2] << " yTiltEnc: " << ytiltResult_1[i][3];
//    }
//    for (int i = 0; i < dataCount; i++){
//        qCInfo(logCate()) << "ytiltResult_2 xTilt: " << ytiltResult_2[i][0] << " yTilt: " << ytiltResult_2[i][1] << " xTiltEnc: " << ytiltResult_2[i][2] << " yTiltEnc: " << ytiltResult_2[i][3];
//    }

}

void AACore::performOCCalibration()
{
    QString informationMessage;
    informationMessage.append(tr("Perform OC Calibration? "));
    if (!UIOperation::getIns()->okCancelConfirm(informationMessage))
    {
        return;
    }
    qCInfo(logCate()) << "Perform OC Calibration";
    QPointF p1, p2, p3, p4;
    //Move 4 points to calculate the sensor pixel to um ratio
    double step = 0.1;
    {
        this->sUTXYZ->xAxis()->relMove(step);
        QThread::msleep(1000);
        cv::Mat input_img = xfGrabber->grabImageCV();
        if (input_img.empty()) { qCWarning(logCate()) << tr("OC Calibration fail due to grab image fail"); }
        QString imageName;
        imageName.append(getDir(AALog::GrabberLogDir))
                .append(getCurrentTimeString())
                .append(".jpg");
        cv::imwrite(imageName.toStdString().c_str(), input_img);
        pattern_v.clear();
        AA_Helper::AAA_Search_MTF_Pattern_Ex_New(input_img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea(), -1, pattern_v);
        p1 = pattern_v.at(0).center;
        pattern_v.clear();
    }
    {
        this->sUTXYZ->xAxis()->relMove(-2*step);
        QThread::msleep(1000);
        cv::Mat input_img = xfGrabber->grabImageCV();
        if (input_img.empty()) { qCWarning(logCate()) << tr("OC Calibration fail due to grab image fail"); }
        QString imageName;
        imageName.append(getDir(AALog::GrabberLogDir))
                .append(getCurrentTimeString())
                .append(".jpg");
        cv::imwrite(imageName.toStdString().c_str(), input_img);
        AA_Helper::AAA_Search_MTF_Pattern_Ex_New(input_img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea(), -1, pattern_v);
        p2 = pattern_v.at(0).center;
        pattern_v.clear();
    }
    {
        this->sUTXYZ->xAxis()->relMove(step);
        this->sUTXYZ->yAxis()->relMove(step);
        QThread::msleep(1000);
        cv::Mat input_img = xfGrabber->grabImageCV();
        if (input_img.empty()) { qCWarning(logCate()) << tr("OC Calibration fail due to grab image fail"); }
        QString imageName;
        imageName.append(getDir(AALog::GrabberLogDir))
                .append(getCurrentTimeString())
                .append(".jpg");
        cv::imwrite(imageName.toStdString().c_str(), input_img);
        AA_Helper::AAA_Search_MTF_Pattern_Ex_New(input_img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea(), -1, pattern_v);
        p3 = pattern_v.at(0).center;
        pattern_v.clear();
    }
    {
        this->sUTXYZ->yAxis()->relMove(-2*step);
        QThread::msleep(1000);
        cv::Mat input_img = xfGrabber->grabImageCV();
        if (input_img.empty()) { qCWarning(logCate()) << tr("OC Calibration fail due to grab image fail"); }
        QString imageName;
        imageName.append(getDir(AALog::GrabberLogDir))
                .append(getCurrentTimeString())
                .append(".jpg");
        cv::imwrite(imageName.toStdString().c_str(), input_img);
        AA_Helper::AAA_Search_MTF_Pattern_Ex_New(input_img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea(), -1, pattern_v);
        p4 = pattern_v.at(0).center;
        pattern_v.clear();
    }

    //p1 p2 p3 p4 are 4 points of the table calibration
    qCInfo(logCate()) << "p1 x: " << p1.x() << " y: " << p1.y();

    qCInfo(logCate()) << "p2 x: " << p2.x() << " y: " << p2.y();

    qCInfo(logCate()) << "p3 x: " << p2.x() << " y: " << p3.y();

    qCInfo(logCate()) << "p4 x: " << p2.x() << " y: " << p4.y();

    QString resultString = "";
    //ToDo: Update sensor X ratio and y ratio
    double sensorXRatio = fabs(p1.x() - p2.x())/(step*2);
    double sensorYRatio = fabs(p3.y() - p4.y())/(step*2);
    resultString.append("Sensor X Ratio: ").append(QString::number(sensorXRatio))
                .append(" Sensor Y Ratio: ").append(QString::number(sensorYRatio));
    if (!UIOperation::getIns()->yesNoConfirm(resultString))
    {
        return;
    }
    aaCoreConfig->setSensorXRatio(sensorXRatio);
    aaCoreConfig->setSensorYRatio(sensorYRatio);
}

void AACore::performInitCameraMaunal()
{
    qCInfo(logCate()) << "Init Camera";
    xfGrabber->enumerateDevice();
    xfGrabber->openDevice();
    xfGrabber->startCamera();
    beginThread();
}

bool AACore::performInitCamera(QJsonValue params)
{
    qCInfo(logCate()) << "Init Camera";
    try {
        xfGrabber->enumerateDevice();
        xfGrabber->openDevice();
        xfGrabber->startCamera();
        beginThread();
    } catch (SilicoolException &se) {
        qCWarning(logCate()) << se.what();
        return false;
    }
    return true;
}

void AACore::performGrabImage()
{
    cv::Mat inputImage = xfGrabber->grabImageCV();
    QString imageName;
    imageName.append(getDir(AALog::GrabberLogDir))
            .append(getCurrentTimeString())
            .append(".jpg");
    cv::imwrite(imageName.toStdString().c_str(), inputImage);
    UIOperation::getIns()->showMessage("Save Image", imageName.append(" saved"), MsgBoxIcon::Information, "OK");
}

void AACore::performCloseCamera()
{
    endThread();
    xfGrabber->closeDevice();
}

void AACore::performOCManual()
{
    QJsonValue params; //Use default value
    performOC(params);
}

bool AACore::performOC(QJsonValue params)
{
    bool enable_motion = params["enable_motion"].toInt(1);
    bool fast_mode = params["fast_mode"].toInt();
    int finish_delay = params["delay_in_ms"].toInt();
    int mode = params["mode"].toInt();  //0: Pattern ; else : Mass center
    int oc_intensity_threshold = params["oc_intensity_threshold"].toInt(0);
    qInfo("enable_motion: %d", enable_motion);
    QVariantMap map;
    //cv::Mat input_img = cv::imread("E:\\tools\\test_img\\image\\image.bmp");
    cv::Mat input_img = xfGrabber->grabImageCV();
    if (input_img.empty()) {
        qCWarning(logCate()) << tr("OC Fail due to grab image fail");
        return false;
    }
    {
        SCTimer t(QString("Image format convertion"), aaCoreTimeLog());
        pattern_v.clear();
        AA_Helper::AAA_Search_MTF_Pattern_Ex_New(input_img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea(), -1, pattern_v);
    }
    if (pattern_v.size() == 0 ) {
        qCWarning(logCate()) << tr("OC Fail due to cannot find any mark pattern");
        return false;
    }
    for (int i = 0; i < pattern_v.size(); i++) {
        if (pattern_v.at(i).layer == 0) {
            qCInfo(logCate()) << "x: " << pattern_v.at(i).center.x() << " y: " << pattern_v.at(i).center.y();
            break;
        }
    }
    if (enable_motion)
    {
        double offsetX = pattern_v.at(0).center.x() - input_img.cols/2;
        double offsetY = pattern_v.at(0).center.y() - input_img.rows/2;
        double stepX = offsetX/aaCoreConfig->SensorXRatio();
        double stepY = offsetY/aaCoreConfig->SensorYRatio();
        qCInfo(logCate()) << "stepX: " << stepX << " stepY: " << stepY;
        sUTXYZ->xAxis()->relMove(stepX);
        sUTXYZ->yAxis()->relMove(stepY);
        QThread::msleep(finish_delay);
    }
    QImage qImage;
    {
        SCTimer t(QString("Image format convertion"), aaCoreTimeLog());
        qImage = cvMat2QImage(input_img);
        ocImageProvider->setImage(qImage);
        emit displayAAImageInUI(AAImageDefinition::OCImage);
    }
    return true;
}

void AACore::performMTFLoopTest()
{
    if (isMTFLoopTestRunning) return;
    else isMTFLoopTestRunning = true;
    while(isMTFLoopTestRunning){
        QJsonValue params;
        performMTF(params);
        QThread::msleep(500);
    }
}

void AACore::performClearMTFLoopTestResult()
{
    mtfLoopTestData.clear();
}

void AACore::performStopMTFLoopTest() {
    isMTFLoopTestRunning = false;
}

void AACore::performMTFManual()
{
    QJsonValue params;
    performMTF(params);
}

bool AACore::performMTF(QJsonValue params)
{
    QVariantMap map;

    double sfr_dev_tol = params["SFR_DEV_TOL"].toDouble(100);
    double corner_dev_tol = params["CORNER_DEV_TOL"].toDouble(100);
    double sfr_tol[16] = {0};
    sfr_tol[0] = params["CC_MIN"].toDouble(0);
    sfr_tol[1] = params["L1_MIN"].toDouble(0);
    sfr_tol[2] = params["L2_MIN"].toDouble(0);
    sfr_tol[3] = params["L3_MIN"].toDouble(0);
    sfr_tol[4] = params["CC_MAX"].toDouble(100);
    sfr_tol[5] = params["L1_MAX"].toDouble(100);
    sfr_tol[6] = params["L2_MAX"].toDouble(100);
    sfr_tol[7] = params["L3_MAX"].toDouble(100);
    sfr_tol[8] = params["CC_AVG_MIN"].toDouble(0);
    sfr_tol[9] = params["L1_AVG_MIN"].toDouble(0);
    sfr_tol[10] = params["L2_AVG_MIN"].toDouble(0);
    sfr_tol[11] = params["L3_AVG_MIN"].toDouble(0);
    sfr_tol[12] = params["CC_AVG_MAX"].toDouble(100);
    sfr_tol[13] = params["L1_AVG_MAX"].toDouble(100);
    sfr_tol[14] = params["L2_AVG_MAX"].toDouble(100);
    sfr_tol[15] = params["L3_AVG_MAX"].toDouble(100);


    SCTimer t(QString("MTF"), aaCoreTimeLog());
    cv::Mat img = xfGrabber->grabImageCV();
    if (img.empty()) {
        return false;
    }
    //cv::Mat img = cv::imread("C:\\Users\\emil\\Desktop\\AAProject\\newSingleHead\\test.jpg");
    int resize_factor = aaCoreConfig->Oversampling() + 1;
    cv::Mat input_img;
    cv::Size size(img.cols/resize_factor, img.rows/resize_factor);
    cv::resize(img, input_img, size);
    {
        {
            SCTimer t(QString("MTF Pattern"), aaCoreTimeLog());
                    pattern_v.clear();
            AA_Helper::AAA_Search_MTF_Pattern_Ex_New(input_img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea()/(resize_factor*resize_factor), aaCoreConfig->MaxArea()/(resize_factor*resize_factor), -1, pattern_v);

            if (pattern_v.size() == 0) {
                qCWarning(logCate()) << tr("Cannot find any mtf pattern");
                return false;
            }
        }

        vector<double> sfr_l_v, sfr_r_v, sfr_t_v, sfr_b_v;
        cv::Rect roi; roi.width = 32; roi.height = 32;
        double rect_width = 0;
        std::vector<MTF_Pattern_Position> vec;
        double imageCenterX = input_img.cols/2;
        double imageCenterY = input_img.rows/2;
        double r1 = sqrt(imageCenterX*imageCenterX + imageCenterY*imageCenterY);
        int samplingFreq = aaCoreConfig->SampleFrequency() + 1;
        int maxLayer = 0;
        {
            for (int i = 0; i < pattern_v.size(); i++) {
                cv::Mat copped_roi;
                SCTimer t(QString("MTF calculation"), aaCoreTimeLog());
                //Crop ROI
                {
                    cv::Rect roi;
                    double width = sqrt(pattern_v[i].area)/2;
                    rect_width = width;
                    roi.width = width*4; roi.height = width*4;
                    roi.x = pattern_v[i].center.x() - width*2;
                    roi.y = pattern_v[i].center.y() - width*2;
                    if (roi.x < 0) roi.x = 0;
                    if (roi.x + roi.width > input_img.cols) { roi.width = input_img.cols - roi.x; }
                    if (roi.y < 0) roi.y = 0;
                    if (roi.y + roi.height > input_img.rows) { roi.height = input_img.rows - roi.y; }
                    input_img(roi).copyTo(copped_roi);
                }
                double sfr_t = 0, sfr_r = 0, sfr_b = 0, sfr_l = 0;
                sfr::sfr_calculation_single_pattern(copped_roi, sfr_t, sfr_r, sfr_b, sfr_l, 8*samplingFreq);
                sfr_t *= 100; sfr_r *= 100; sfr_b *= 100; sfr_l *= 100;
                double avg_sfr = (sfr_l + sfr_l + sfr_l + sfr_l) / 4;
                double radius = sqrt(pow(pattern_v[i].center.x() - imageCenterX, 2) + pow(pattern_v[i].center.y() - imageCenterY, 2));
                double f = radius/r1;
                if (pattern_v[i].layer > maxLayer) maxLayer = pattern_v[i].layer;
                qCInfo(logCate()) << tr("x: %1 y: %2 sfr_avg: %3 layer: %4")
                                     .arg(pattern_v[i].center.x()).arg(pattern_v[i].center.y())
                                     .arg(avg_sfr).arg(pattern_v[i].layer);
                vec.emplace_back(pattern_v[i].center.x(), pattern_v[i].center.y(),
                                         f, sfr_t, sfr_r, sfr_b, sfr_l, pattern_v[i].area, avg_sfr);
            }
        }

        //Draw MTF Result
        {
            QImage qImage;
            {
                SCTimer t(QString("Image format convertion"), aaCoreTimeLog());
                qImage = cvMat2QImage(input_img);
            }
            QPainter qPainter(&qImage);
            SCTimer t(QString("MTF Draw Result"), aaCoreTimeLog());
            qPainter.setBrush(Qt::NoBrush);
            qPainter.setFont(QFont("Times", aaCoreConfig->FontSizeDisplay(), QFont::Light));
            for (size_t i = 0; i < vec.size(); i++) {
                qPainter.setPen(QPen(Qt::magenta, 4.0));
                qPainter.drawText(vec[i].x - rect_width/2, vec[i].y - rect_width, QString::number(vec[i].t_sfr, 'g', 4));
                qPainter.drawText(vec[i].x + 150, vec[i].y,  QString::number(vec[i].r_sfr, 'g', 4));
                qPainter.drawText(vec[i].x - 50, vec[i].y + rect_width,  QString::number(vec[i].b_sfr, 'g', 4));
                qPainter.drawText(vec[i].x - 250, vec[i].y,  QString::number(vec[i].l_sfr, 'g', 4));
            }
            qPainter.end();
            mtfImageProvider->setImage(qImage);
            emit displayAAImageInUI(AAImageDefinition::MTFImage);
        }

        //MTF Check Specification
        bool sfr_check = true;
        // Check 4 lines in CC if each SFR score is lower than min or larger than max
        if (vec[0].t_sfr < sfr_tol[0] || vec[0].r_sfr < sfr_tol[0] || vec[0].b_sfr < sfr_tol[0] || vec[0].l_sfr < sfr_tol[0]
            || vec[0].t_sfr > sfr_tol[4] || vec[0].r_sfr > sfr_tol[4] || vec[0].b_sfr > sfr_tol[4] || vec[0].l_sfr > sfr_tol[4])
        {
            qCWarning(logCate()) << "Perform MTF Check fail. Reason: CC Fail";
            sfr_check = false;
        }
        //Check avg_sfr in CC if lower than min or larger than max
        if (vec[0].avg_sfr < sfr_tol[8] || vec[0].avg_sfr > sfr_tol[12])
        {
            qCWarning(logCate()) << "Perform MTF Check fail. Reason: Average CC Fail";
            sfr_check = false;
        }
        qCInfo(logCate()) << "CC - t_sfr: " <<vec[0].t_sfr << " r_sfr: " << vec[0].r_sfr << " b_sfr: " << vec[0].b_sfr << " l_sfr: " << vec[0].l_sfr;

        for(int i = 0; i <= maxLayer-1; i++) {
            if (vec.size() < i*4 + 4) {
                qCWarning(logCate()) << tr("Cannot find UR / UL / LL / LR in the image");
                sfr_check = false;
                break;
            }
            map.insert(QString("UL_T_SFR_").append(QString::number(i+1)), round(vec[i*4 + 1].t_sfr*1000)/1000);
            map.insert(QString("UL_R_SFR_").append(QString::number(i+1)), round(vec[i*4 + 1].r_sfr*1000)/1000);
            map.insert(QString("UL_B_SFR_").append(QString::number(i+1)), round(vec[i*4 + 1].b_sfr*1000)/1000);
            map.insert(QString("UL_L_SFR_").append(QString::number(i+1)), round(vec[i*4 + 1].l_sfr*1000)/1000);
            map.insert(QString("UL_SFR_").append(QString::number(i+1)), round(vec[i*4 + 1].avg_sfr*1000)/1000);
            map.insert(QString("LL_T_SFR_").append(QString::number(i+1)), round(vec[i*4 + 2].t_sfr*1000)/1000);
            map.insert(QString("LL_R_SFR_").append(QString::number(i+1)), round(vec[i*4 + 2].r_sfr*1000)/1000);
            map.insert(QString("LL_B_SFR_").append(QString::number(i+1)), round(vec[i*4 + 2].b_sfr*1000)/1000);
            map.insert(QString("LL_L_SFR_").append(QString::number(i+1)), round(vec[i*4 + 2].l_sfr*1000)/1000);
            map.insert(QString("LL_SFR_").append(QString::number(i+1)), round(vec[i*4 + 2].avg_sfr*1000)/1000);
            map.insert(QString("LR_T_SFR_").append(QString::number(i+1)), round(vec[i*4 + 3].t_sfr*1000)/1000);
            map.insert(QString("LR_R_SFR_").append(QString::number(i+1)), round(vec[i*4 + 3].r_sfr*1000)/1000);
            map.insert(QString("LR_B_SFR_").append(QString::number(i+1)), round(vec[i*4 + 3].b_sfr*1000)/1000);
            map.insert(QString("LR_L_SFR_").append(QString::number(i+1)), round(vec[i*4 + 3].l_sfr*1000)/1000);
            map.insert(QString("LR_SFR_").append(QString::number(i+1)), round(vec[i*4 + 3].avg_sfr*1000)/1000);
            map.insert(QString("UR_T_SFR_").append(QString::number(i+1)), round(vec[i*4 + 4].t_sfr*1000)/1000);
            map.insert(QString("UR_R_SFR_").append(QString::number(i+1)), round(vec[i*4 + 4].r_sfr*1000)/1000);
            map.insert(QString("UR_B_SFR_").append(QString::number(i+1)), round(vec[i*4 + 4].b_sfr*1000)/1000);
            map.insert(QString("UR_L_SFR_").append(QString::number(i+1)), round(vec[i*4 + 4].l_sfr*1000)/1000);
            map.insert(QString("UR_SFR_").append(QString::number(i+1)), round(vec[i*4 + 4].avg_sfr*1000)/1000);
            //Check each 4 ROI in 03F, 05F, 08F if each 4 SFR score is lower than min or larger than max
            if (vec[i*4+1].t_sfr < sfr_tol[i+1] || vec[i*4+1].r_sfr < sfr_tol[i+1] || vec[i*4+1].b_sfr < sfr_tol[i+1] || vec[i*4+1].l_sfr < sfr_tol[i+1]
                    || vec[i*4+2].t_sfr < sfr_tol[i+1] || vec[i*4+2].r_sfr < sfr_tol[i+1] || vec[i*4+2].b_sfr < sfr_tol[i+1] || vec[i*4+2].l_sfr < sfr_tol[i+1]
                    || vec[i*4+3].t_sfr < sfr_tol[i+1] || vec[i*4+3].r_sfr < sfr_tol[i+1] || vec[i*4+3].b_sfr < sfr_tol[i+1] || vec[i*4+3].l_sfr < sfr_tol[i+1]
                    || vec[i*4+4].t_sfr < sfr_tol[i+1] || vec[i*4+4].r_sfr < sfr_tol[i+1] || vec[i*4+4].b_sfr < sfr_tol[i+1] || vec[i*4+4].l_sfr < sfr_tol[i+1]
                    || vec[i*4+1].t_sfr > sfr_tol[i+5] || vec[i*4+1].r_sfr > sfr_tol[i+5] || vec[i*4+1].b_sfr > sfr_tol[i+5] || vec[i*4+1].l_sfr > sfr_tol[i+5]
                    || vec[i*4+2].t_sfr > sfr_tol[i+5] || vec[i*4+2].r_sfr > sfr_tol[i+5] || vec[i*4+2].b_sfr > sfr_tol[i+5] || vec[i*4+2].l_sfr > sfr_tol[i+5]
                    || vec[i*4+3].t_sfr > sfr_tol[i+5] || vec[i*4+3].r_sfr > sfr_tol[i+5] || vec[i*4+3].b_sfr > sfr_tol[i+5] || vec[i*4+3].l_sfr > sfr_tol[i+5]
                    || vec[i*4+4].t_sfr > sfr_tol[i+5] || vec[i*4+4].r_sfr > sfr_tol[i+5] || vec[i*4+4].b_sfr > sfr_tol[i+5] || vec[i*4+4].l_sfr > sfr_tol[i+5])
            {
                //qInfo("Layer %d check SFR fail with %f,%f",i+1,sfr_tol[i+1],sfr_tol[i+5]);
                if (i == 0) {
                    qWarning(logCate()) << tr("MTF Check Fail. Layer 1 fail");
                }
                else if (i == 1) {
                    qWarning(logCate()) << tr("MTF Check Fail. Layer 2 fail");
                }
                else if (i == 2) {
                    qWarning(logCate()) << tr("MTF Check Fail. Layer 3 fail");
                }
                sfr_check = false;
            }

            //Check if average sfr is lower than min or larger than max
            if (vec[i*4+1].avg_sfr < sfr_tol[i+9] || vec[i*4+1].avg_sfr > sfr_tol[i+13])
            {
                qInfo("Layer %d check average SFR fail with %f,%f", sfr_tol[i+9],sfr_tol[i+13]);
                if (i == 0) {
                    qWarning(logCate()) << tr("MTF Check Fail. Average MTF in Layer 1 fail");
                }
                else if (i == 1) {
                    qWarning(logCate()) << tr("MTF Check Fail. Average MTF in Layer 2 fail");
                }
                else if (i == 2) {
                    qWarning(logCate()) << tr("MTF Check Fail. Average MTF in Layer 3 fail");
                }
                sfr_check = false;
            }

            qCInfo(logCate()) << "UL - t_sfr: " <<vec[i*4 + 1].t_sfr << " r_sfr: " << vec[i*4 + 1].r_sfr << " b_sfr: " << vec[i*4 + 1].b_sfr << " l_sfr: " << vec[i*4 + 1].l_sfr;
            qCInfo(logCate()) << "UR - t_sfr: " <<vec[i*4 + 2].t_sfr << " r_sfr: " << vec[i*4 + 2].r_sfr << " b_sfr: " << vec[i*4 + 2].b_sfr << " l_sfr: " << vec[i*4 + 2].l_sfr;
            qCInfo(logCate()) << "LR - t_sfr: " <<vec[i*4 + 3].t_sfr << " r_sfr: " << vec[i*4 + 3].r_sfr << " b_sfr: " << vec[i*4 + 3].b_sfr << " l_sfr: " << vec[i*4 + 3].l_sfr;
            qCInfo(logCate()) << "LL - t_sfr: " <<vec[i*4 + 4].t_sfr << " r_sfr: " << vec[i*4 + 4].r_sfr << " b_sfr: " << vec[i*4 + 4].b_sfr << " l_sfr: " << vec[i*4 + 4].l_sfr;

        }

        //Post MTF Loop Test Result
        if (true) {
            double ccSFR = 0, ulSFR = 0, urSFR = 0, llSFR = 0, lrSFR = 0;
            if (vec.size() > 0) ccSFR = vec[0].avg_sfr;
            if (vec.size() > (maxLayer-1)*4 + 1) ulSFR = vec[(maxLayer-1)*4 + 1].avg_sfr;
            if (vec.size() > (maxLayer-1)*4 + 4) urSFR = vec[(maxLayer-1)*4 + 4].avg_sfr;
            if (vec.size() > (maxLayer-1)*4 + 2) lrSFR = vec[(maxLayer-1)*4 + 2].avg_sfr;
            if (vec.size() > (maxLayer-1)*4 + 3) llSFR = vec[(maxLayer-1)*4 + 3].avg_sfr;
            this->mtfLoopTestData.incrementData(ccSFR, ulSFR, urSFR, lrSFR, llSFR);
        }
        pattern_v.clear();

        if (!sfr_check) {
            if (this->currentAAResult.productState == AAResult::GoodProduct
             || this->currentAAResult.productState == AAResult::NgProduct) {
                this->setNgProduct();
            } else {
                this->setNgLens();
            }
            return false;
        }
    }
    return true;
}

bool AACore::performDelay(QJsonValue params)
{
    int delay_in_ms = params["delay_in_ms"].toInt();
    SCTimer t(QString("Delay"), aaCoreTimeLog());
    QThread::msleep(delay_in_ms);
    return true;
}

bool AACore::performAA(QJsonValue params)
{
    //return performAAOffline(params);
    qCInfo(logCate()) << tr("perform AA");
    SCTimer t("aaOffline", aaCoreTimeLog());
    clustered_sfr_map.clear();
    QVariantMap map, stepTimerMap, dFovMap, sfrTimeElapsedMap;

    int zScanMode = params["mode"].toInt();

    double start = params["aa1_start_pos"].toDouble();
    double stop = params["aa1_stop_pos"].toDouble();
    double step_size = params["step_size"].toDouble()/1000;
    unsigned int zSleepInMs = params["delay_Z_in_ms"].toInt();
    double offset_in_um = params["offset_in_um"].toDouble()/1000;
    int enableTilt = params["enable_tilt"].toInt();
    int imageCount = params["image_count"].toInt();
    int positionChecking = params["position_checking"].toInt();
    int isDebug = params["is_debug"].toInt();
    int resize_factor = 1;
    int sfrCount = 0;
    double xsum=0,x2sum=0,ysum=0,xysum=0;
    double estimated_fov_slope = 15;

    qCInfo(logCate()) << "step_size: " << step_size << " start pos: " << start << " stop pos: " << stop << " isDebug: " << isDebug;

    if (zScanMode == ZSCAN_MODE::AA_ZSCAN_NORMAL) {
        for (int i = 0; i < imageCount; i++)
        {
            this->sUTXYZ->zAxis()->absMove(start + step_size*i);
            QThread::msleep(zSleepInMs);
            cv::Mat input_img = xfGrabber->grabImageCV();
            if (input_img.empty()) {
                qCWarning(logCate()) << tr("Perform AA fail due to cannot grab image");
                return false;
            }
            resize_factor = aaCoreConfig->Oversampling() + 1;
            cv::Mat img;
            cv::Size size(input_img.cols/resize_factor, input_img.rows/resize_factor);
            cv::resize(input_img, img, size);
            if (isDebug) {
                QString imageName;
                imageName.append(getDir(AALog::ZScanLogDir))
                        .append(getCurrentTimeString())
                        .append("_aa_scan_").append(QString::number(i))
                        .append(".jpg");
                cv::imwrite(imageName.toStdString().c_str(), img);
            }
            emit sfrWorkerController->calculate(i, this->sUTXYZ->zAxis()->getFeedbackPos(), img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea(), aaCoreConfig->SampleFrequency()+1, aaCoreConfig->AASelectedLayer()+1);
            img.release();
            sfrCount++;
            qCInfo(logCate()) << "Z Scan finish : " << i;
        }
    } else if (zScanMode == ZSCAN_MODE::AA_DFOV_MODE) {
        this->sUTXYZ->zAxis()->absMove(start);
        QThread::msleep(zSleepInMs);
        cv::Mat input_img = xfGrabber->grabImageCV();
        if (input_img.empty()) {
            qCWarning(logCate()) << tr("Perform AA fail due to cannot grab image");
            return false;
        }
        double dfov = calculateDFOV(input_img);
        if (dfov < 0) {
            qCWarning(logCate()) << tr("Perform AA fail due to cannot calculate DFOV");
            return false;
        }
        double estimated_aa_z = (aaCoreConfig->EstimatedAAFOV() - dfov)/aaCoreConfig->FOVSlope() + this->sUTXYZ->zAxis()->getFeedbackPos();
        double target_z = estimated_aa_z + offset_in_um;
        qInfo("The estimated target z is: %f dfov is %f", target_z, dfov);
        for (unsigned int i = 0; i < imageCount; i++) {
            this->sUTXYZ->zAxis()->absMove(target_z + step_size*i);
            QThread::msleep(zSleepInMs);
            cv::Mat input_img = xfGrabber->grabImageCV();
            if (input_img.empty()) {
                qCWarning(logCate()) << tr("Perform AA fail due to cannot grab image");
                return false;
            }
            resize_factor = aaCoreConfig->Oversampling() + 1;
            cv::Mat img;
            cv::Size size(input_img.cols/resize_factor, input_img.rows/resize_factor);
            cv::resize(input_img, img, size);
            if (isDebug) {
                QString imageName;
                imageName.append(getDir(AALog::ZScanLogDir))
                        .append(getCurrentTimeString())
                        .append("_aa_scan_").append(QString::number(i))
                        .append(".jpg");
                cv::imwrite(imageName.toStdString().c_str(), img);
            }
            emit sfrWorkerController->calculate(i, this->sUTXYZ->zAxis()->getFeedbackPos(), img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea(), aaCoreConfig->SampleFrequency()+1, aaCoreConfig->AASelectedLayer()+1);
            img.release();
            sfrCount++;
            qCInfo(logCate()) << "Z Scan finish : " << i;
        }
    } else if (zScanMode == ZSCAN_MODE::AA_STATIONARY_SCAN_MODE) {
        double currentZ =  this->sUTXYZ->zAxis()->getFeedbackPos();
        double startZ = currentZ + offset_in_um;
        qCInfo(logCate()) << "Stationary mode - start Z : " << startZ;
        for (unsigned int i = 0; i < imageCount; i++) {
            this->sUTXYZ->zAxis()->absMove(startZ + step_size*i);
            QThread::msleep(zSleepInMs);
            cv::Mat input_img = xfGrabber->grabImageCV();
            if (input_img.empty()) {
                qCWarning(logCate()) << tr("Perform AA fail due to cannot grab image");
                return false;
            }
            resize_factor = aaCoreConfig->Oversampling() + 1;
            cv::Mat img;
            cv::Size size(input_img.cols/resize_factor, input_img.rows/resize_factor);
            cv::resize(input_img, img, size);
            if (isDebug) {
                QString imageName;
                imageName.append(getDir(AALog::ZScanLogDir))
                        .append(getCurrentTimeString())
                        .append("_aa_scan_").append(QString::number(i))
                        .append(".jpg");
                cv::imwrite(imageName.toStdString().c_str(), img);
            }
            emit sfrWorkerController->calculate(i, this->sUTXYZ->zAxis()->getFeedbackPos(), img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea(), aaCoreConfig->SampleFrequency()+1, aaCoreConfig->AASelectedLayer()+1);
            img.release();
            sfrCount++;
            qCInfo(logCate()) << "Z Scan finish : " << i;
        }
    }

    int timeout=2000;
    while(this->clustered_sfr_map.size() != sfrCount && timeout > 0) {
        Sleep(10);
        timeout--;
    }
    if (timeout <= 0) {
        qCCritical(logCate())<< tr("Error in performing AA. Timeout reached: %1").arg(timeout);
        return false;
    }
    QVariantMap aa_result = sfrFitCurve_Advance(resize_factor, start);

    if (enableTilt == 1) {
        double x_tilt_offset = aa_result["yTilt"].toDouble()/aaCoreConfig->aaCalibrationYTilt();
        double y_tilt_offset = aa_result["xTilt"].toDouble()/aaCoreConfig->aaCalibrationXTilt();
        this->aaThetaXYZ->xAxis()->relMove(x_tilt_offset);
        this->aaThetaXYZ->yAxis()->relMove(y_tilt_offset);
    }
    this->sUTXYZ->zAxis()->absMove(aa_result["zPeak"].toDouble());
    clustered_sfr_map.clear();

    return true;
}

void AACore::runFlowchart()
{
    SCTimer t("runFlowchart", aaCoreTimeLog());
    QVariantMap jsonMap = currentFlowchartDocument.object().toVariantMap();
    QJsonObject links = jsonMap["links"].toJsonObject();
    QJsonObject operators = jsonMap["operators"].toJsonObject();
    bool end = false;
    QString currentPointer = "start";
    while (!end)
    {
        end = true;
        foreach(const QString &key, links.keys()){
            QJsonValue value = links.value(key);
            if (value["fromOperator"].toString() == currentPointer
                    && value["fromConnector"].toString() == "success") {
                if (currentPointer == "start") {
                    qInfo(QString("Move from " + currentPointer + " to " + value["toOperator"].toString()).toStdString().c_str());
                    currentPointer = value["toOperator"].toString();
                    if (links.size() == 1) {
                        QJsonValue op = operators[currentPointer.toStdString().c_str()];
                        performTest(currentPointer.toStdString().c_str(), op["properties"]);
                    }
                    end = false;
                    break;
                }
                else {
                    qInfo(QString("Do Test:" + currentPointer).toStdString().c_str());
                    QJsonValue op = operators[currentPointer.toStdString().c_str()];
                    bool ret = performTest(currentPointer.toStdString().c_str(), op["properties"]);
                    if (ret) {
                        currentPointer = value["toOperator"].toString();
                    } else {
                        //Find fail path
                        bool hasFailPath = false;
                        foreach(const QString &key, links.keys()){
                            QJsonValue value = links.value(key);
                            if (value["fromOperator"].toString() == currentPointer
                                    && value["fromConnector"].toString() == "fail") {
                                currentPointer = value["toOperator"].toString();
                                hasFailPath = true;
                                break;
                            }
                        }
                        if (!hasFailPath) {
                            qInfo("Finished With Auto Reject");
                            //performReject();
                            end = true;
                            break;
                        }
                    }
                    if (currentPointer.contains("Accept")
                            ||currentPointer.contains("Reject")
                            ||currentPointer.contains("Terminate")) {
                        QJsonValue op = operators[currentPointer.toStdString().c_str()];
                        //ErrorCodeStruct ret_error = performTest(currentPointer.toStdString().c_str(), op["properties"]);
                        performTest(currentPointer.toStdString().c_str(), op["properties"]);
                        qInfo("Finished With %s",currentPointer.toStdString().c_str());
                        end = true;
                        break;
                    }
                    end = false;
                    break;
                }
            } else if ( value["fromOperator"].toString() == currentPointer
                        && value["fromConnector"].toString() == "thread_1" ) {
                qInfo("Found Parallel Test Item");
                std::vector<QString> thread_1_test_list, thread_2_test_list;

                QString current_thread_1 = currentPointer, current_thread_2 = currentPointer;
                //Find the head first
                bool isFoundThread_1 = false, isFoundThread_2 = false;
                foreach(const QString &key, links.keys()){
                    QJsonValue value = links.value(key);
                    if (value["fromOperator"].toString() == current_thread_1 && value["fromConnector"] == "thread_1") {
                        current_thread_1 = value["toOperator"].toString();
                        isFoundThread_1 = true;
                    } else if (value["fromOperator"].toString() == current_thread_2 && value["fromConnector"] == "thread_2") {
                        current_thread_2 = value["toOperator"].toString();
                        isFoundThread_2 = true;
                    }
                    if (isFoundThread_1 && isFoundThread_2) {
                        thread_1_test_list.push_back(current_thread_1);
                        thread_2_test_list.push_back(current_thread_2);
                        break;
                    }
                }
                //Traverse the thread
                bool thread1End = false;
                while (!thread1End)
                {
                    thread1End = true;
                    foreach(const QString &key, links.keys()){
                        QJsonValue value = links.value(key);
                        if (value["fromOperator"].toString() == current_thread_1) {
                            thread_1_test_list.push_back(value["toOperator"].toString());
                            current_thread_1 = value["toOperator"].toString();
                            thread1End = false;
                            break;
                        }
                    }
                    if (current_thread_1.contains("Join")) {
                        thread1End = true;
                    }
                }
                bool thread2End = false;
                while (!thread2End)
                {
                    thread2End = true;
                    foreach(const QString &key, links.keys()){
                        QJsonValue value = links.value(key);
                        if (value["fromOperator"].toString() == current_thread_2) {
                            thread_2_test_list.push_back(value["toOperator"].toString());
                            current_thread_2 = value["toOperator"].toString();
                            thread2End = false;
                            if (current_thread_2.contains("Join")) {
                                currentPointer = current_thread_2;
                                thread2End = true;
                            }
                            break;
                        }
                    }
                }
                qCInfo(logCate()) << tr("End of traversal: %s").arg(jsonMap["operators"].toString().toStdString().c_str());
                QJsonValue op1 = operators[thread_1_test_list[0].toStdString().c_str()];
                QJsonValue op2 = operators[thread_2_test_list[0].toStdString().c_str()];
                //ErrorCodeStruct ret = performParallelTest(thread_1_test_list, thread_2_test_list, op1["properties"], op2["properties"]);
                //if (ret.code != ErrorCode::OK) {
                //    qInfo("Finished With Auto Reject");
                //performReject();
                //    end = true;
                //    break;
                //}
                //Perform Parallel Test
            }
        }
    }
    qCInfo(logCate()) << tr("Finish flowchart cycle");
}

void AACore::runAAFlowchart() {

    qCInfo(logCate()) << tr("Run AA Flowchart");
    runFlowchart();
    //ToDo: Move to test item later
    performCloseCamera();
    sendEvent(WorkerName::AaHead, WorkerEvent("aaFinish"));
}

void AACore::storeSfrResults(unsigned int index, vector<Sfr_entry> sfrs, int timeElasped)
{
    clustered_sfr_map[index] = std::move(sfrs);
    qCInfo(logCate()) << tr("Received sfr result from index: %1 timeElapsed: %2 size: %3")
                         .arg(index)
                         .arg(timeElasped)
                         .arg(clustered_sfr_map.size());
}

void AACore::performManualTest(QString operatorId, QString paramsJson) {
    QString informationMessage;
    informationMessage.append(tr("Perform Manual Test "))
                      .append(operatorId);
    if (!UIOperation::getIns()->okCancelConfirm(informationMessage))
    {
        return;
    }
    QJsonDocument jsonDoc = QJsonDocument::fromJson(paramsJson.toStdString().c_str());
    QJsonValue params = jsonDoc.object();

    if (operatorId.contains(AA_PIECE_OC)) {
        performOC(params);
    } else if (operatorId.contains(AA_PIECE_MTF)) {
        performMTF(params);
    } else if (operatorId.contains(AA_PIECE_AA)) {
        performAA(params);
    } else if (operatorId.contains(AA_PIECE_INIT_CAMERA)) {
        performInitCamera(params);
    } else if (operatorId.contains(AA_PIECE_DISPENSE)) {
        performDispense(params);
    } else if (operatorId.contains(AA_PIECE_UV)) {
        performUV(params);
    }
}

void AACore::performAADebugImage()
{
    QString filename = aaCoreConfig->aaDebugImageFilename(); //pr_name.replace("file:///", "");
    filename.replace("file:///", "");
    QFile file(filename);
    if (!file.exists()) {
        UIOperation::getIns()->showError(tr("Input file does not exist"));
    }
    qCInfo(logCate()) << "Perform AA Debug Image" << " Input file: " << filename;
    AA_Helper::getAADebugImage(filename, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea());
    QImage image("debugImage.jpg");
    aaDebugImageProvider->setImage(image);
    emit displayAAImageInUI(AAImageDefinition::AADebugImage);
}

void AACore::resetAAResultCurrentUnitToRaw()
{
    qCInfo(logCate()) << tr("Reset Unit To Raw");
    currentAAResult.lensState = AAResult::RawLens;
    currentAAResult.sensorState = AAResult::RawSensor;
    currentAAResult.productState = AAResult::NoProduct;
}

void AACore::setNgLens() {
    qCInfo(logCate()) << tr("Set Ng Lens");
    currentAAResult.lensState = AAResult::NgLens;
}

void AACore::setNgSensor() {
    qCInfo(logCate()) << tr("Set Ng Sensor");
    currentAAResult.sensorState = AAResult::NgSensor;
}

void AACore::setNgProduct() {
    qCInfo(logCate()) << tr("Set Ng Product");
    currentAAResult.sensorState = AAResult::NoSensor;
    currentAAResult.lensState = AAResult::NoLens;
    currentAAResult.productState = AAResult::NgProduct;
}

void AACore::setGoodProduct() {
    qCInfo(logCate()) << tr("Set Good Product");
    currentAAResult.sensorState = AAResult::NoSensor;
    currentAAResult.lensState = AAResult::NoLens;
    currentAAResult.productState = AAResult::GoodProduct;
}

AAResultStruct AACore::getAAResultCurrentUnit(){ return currentAAResult; }

QImage AACore::cvMat2QImage(const cv::Mat& mat)
{
    // 8-bits unsigned, NO. OF CHANNELS = 1
    if(mat.type() == CV_8UC1)
    {
        QImage image(mat.cols, mat.rows, QImage::Format_Indexed8);
        // Set the color table (used to translate colour indexes to qRgb values)
        image.setColorCount(256);
        for(int i = 0; i < 256; i++)
        {
            image.setColor(i, qRgb(i, i, i));
        }
        // Copy input Mat
        uchar *pSrc = mat.data;
        for(int row = 0; row < mat.rows; row ++)
        {
            uchar *pDest = image.scanLine(row);
            memcpy(pDest, pSrc, mat.cols);
            pSrc += mat.step;
        }
        return image;
    }
    // 8-bits unsigned, NO. OF CHANNELS = 3
    else if(mat.type() == CV_8UC3)
    {
        // Copy input Mat
        const uchar *pSrc = (const uchar*)mat.data;
        // Create QImage with same dimensions as input Mat
        QImage image(pSrc, mat.cols, mat.rows, mat.step, QImage::Format_RGB888);
        return image.rgbSwapped();
    }
    else if(mat.type() == CV_8UC4)
    {
        // Copy input Mat
        const uchar *pSrc = (const uchar*)mat.data;
        // Create QImage with same dimensions as input Mat
        QImage image(pSrc, mat.cols, mat.rows, mat.step, QImage::Format_ARGB32);
        return image.copy();
    }
    else
    {
        return QImage();
    }
}

bool zPeakComp(const threeDPoint & p1, const threeDPoint & p2){ return p1.z < p2.z; }

QVariantMap AACore::sfrFitCurve_Advance(int resize_factor, double start_pos)
{
    QVariantMap result, map;
    vector<vector<Sfr_entry>> sorted_sfr_map;
    vector<vector<double>> sorted_sfr_fit_map;
    for (size_t i = 0; i < clustered_sfr_map[0].size(); ++i)
    {
        vector<Sfr_entry> sfr_map;
        for (size_t ii = 0; ii < clustered_sfr_map.size(); ++ii)
        {
            sfr_map.push_back(clustered_sfr_map[ii][i]);
        }
        sorted_sfr_map.push_back(sfr_map);
    }
    qCInfo(logCate()) << tr("clustered sfr map pattern size: %1 sorted_sfr_map size: %2")
                         .arg(clustered_sfr_map[0].size())
                         .arg(sorted_sfr_map.size());
    if (clustered_sfr_map[0].size() == 0 || clustered_sfr_map[0].size() < 4) {
        qCWarning(logCate()) << tr("AA Scan Fail. Not enough data points for data fitting");
        result.insert("OK", false);
        return result;
    }
    int fitOrder = aaCoreConfig->FitOrder();
    threeDPoint point_0;
    vector<threeDPoint> points_1, points_11;
    vector<threeDPoint> points_2, points_22;
    vector<threeDPoint> points_3, points_33;
    for (size_t i = 0; i < sorted_sfr_map.size(); i++) {
        vector<double> sfr, z, sfr_fit;
        double ex = 0; double ey = 0;
        for (size_t ii=0; ii < sorted_sfr_map[i].size(); ii++) {
            sfr.push_back(sorted_sfr_map[i][ii].sfr);
            z.push_back(sorted_sfr_map[i][ii].pz-start_pos);
            ex += sorted_sfr_map[i][ii].px*resize_factor;
            ey += sorted_sfr_map[i][ii].py*resize_factor;
        }
        ex /= (sorted_sfr_map[i].size()*aaCoreConfig->SensorXRatio());
        ey /= (sorted_sfr_map[i].size()*aaCoreConfig->SensorYRatio());

        double peak_sfr, peak_z;
        fitCurve(z, sfr, fitOrder, peak_z, peak_sfr, sfr_fit);
        sorted_sfr_fit_map.push_back(sfr_fit);
        if (i==0) {
            point_0.x = ex; point_0.y = ey; point_0.z = peak_z + start_pos;
        } else if ( i >= 1 && i <= 4) {
            points_1.emplace_back(ex, ey, peak_z + start_pos);
            points_11.emplace_back(ex, ey, peak_z + start_pos);
        } else if ( i >= 5 && i <= 8) {
            points_2.emplace_back(ex, ey, peak_z + start_pos);
            points_22.emplace_back(ex, ey, peak_z + start_pos);
        } else if ( i >= 9 && i <= 12) {
            points_3.emplace_back(ex, ey, peak_z + start_pos);
            points_33.emplace_back(ex, ey, peak_z + start_pos);
        }
    }
    sort(points_11.begin(), points_11.end(), zPeakComp);
    sort(points_22.begin(), points_22.end(), zPeakComp);
    sort(points_33.begin(), points_33.end(), zPeakComp);

    qCInfo(logCate()) << tr("Layer 0 - x: %1 y: %2 z: %3")
                         .arg(point_0.x)
                         .arg(point_0.y)
                         .arg(point_0.z);

    int layerDetected = 0; int validLayer = 0;
    if (points_1.size()==4) { validLayer++; }
    if (points_2.size()==4) { validLayer++; }
    if (points_3.size()==4) { validLayer++; }
    for (size_t i = 0; i < points_1.size(); i++) {
        qCInfo(logCate()) << tr("Layer 1 - Index: %1 x: %2 y: %3 z: %4")
                             .arg(i)
                             .arg(points_1[i].x)
                             .arg(points_1[i].y)
                             .arg(points_1[i].z);
        layerDetected++;
    }

    for (size_t i = 0; i < points_2.size(); i++) {
        qCInfo(logCate()) << tr("Layer 2 - Index: %1 x: %2 y: %3 z: %4")
                             .arg(i)
                             .arg(points_2[i].x)
                             .arg(points_2[i].y)
                             .arg(points_2[i].z);
        layerDetected++;
    }

    for (size_t i = 0; i < points_3.size(); i++) {
        qCInfo(logCate()) << tr("Layer 3 - Index: %1 x: %2 y: %3 z: %4")
                             .arg(i)
                             .arg(points_3[i].x)
                             .arg(points_3[i].y)
                             .arg(points_3[i].z);
        layerDetected++;
    }

    //Layer checking
    if ((points_1.size() > 0 && points_1.size() < 4) ||
        (points_2.size() > 0 && points_2.size() < 4) ||
        (points_3.size() > 0 && points_3.size() < 4))
    {
        qCritical(logCate()) << tr("AA pattern layer checking fail.");
        result.insert("OK", false);
        //emit postSfrDataToELK(runningUnit, map);
        return result;
    }

    threeDPoint weighted_vector_1 = aaCorePlaneFitting(points_1);
    threeDPoint weighted_vector_2 = aaCorePlaneFitting(points_2);
    threeDPoint weighted_vector_3 = aaCorePlaneFitting(points_3);
    double dev_1 = 0, dev_2 = 0, dev_3 =0;
    if (points_11.size()>0) dev_1 = fabs(points_11[0].z - points_11[points_11.size()-1].z)*1000;
    if (points_22.size()>0) dev_2 = fabs(points_22[0].z - points_22[points_22.size()-1].z)*1000;
    if (points_33.size()>0) dev_3 = fabs(points_33[0].z - points_33[points_33.size()-1].z)*1000;
    double xTilt_1 = weighted_vector_1.z * weighted_vector_1.x;
    double yTilt_1 = weighted_vector_1.z * weighted_vector_1.y;
    double xTilt_2 = weighted_vector_2.z * weighted_vector_2.x;
    double yTilt_2 = weighted_vector_2.z * weighted_vector_2.y;
    double xTilt_3 = weighted_vector_3.z * weighted_vector_3.x;
    double yTilt_3 = weighted_vector_3.z * weighted_vector_3.y;

    qCInfo(logCate()) << tr("Layer 1: xTilt: %1 yTilt: %2 dev: %3")
                         .arg(xTilt_1)
                         .arg(yTilt_1)
                         .arg(dev_1);
    qCInfo(logCate()) << tr("Layer 2: xTilt: %1 yTilt: %2 dev: %3")
                         .arg(xTilt_2)
                         .arg(yTilt_2)
                         .arg(dev_2);
    qCInfo(logCate()) << tr("Layer 3: xTilt: %1 yTilt: %2 dev: %3")
                         .arg(xTilt_3)
                         .arg(yTilt_3)
                         .arg(dev_3);

    result.insert("xTilt_1", xTilt_1); map.insert("xTilt_1", xTilt_1);
    result.insert("yTilt_1", yTilt_1); map.insert("yTilt_1", yTilt_1);
    result.insert("xTilt_2", xTilt_2); map.insert("xTilt_2", xTilt_2);
    result.insert("yTilt_2", yTilt_2); map.insert("yTilt_2", yTilt_2);
    result.insert("xTilt_3", xTilt_3); map.insert("xTilt_3", xTilt_3);
    result.insert("yTilt_3", yTilt_3); map.insert("yTilt_3", yTilt_3);
    result.insert("zPeak", point_0.z); map.insert("zPeak", point_0.z);
    result.insert("OK", true);

    AAData *data;
    if (currentChartDisplayChannel == 0) {
        data = &aaData_1;
        currentChartDisplayChannel = 1;
    } else {
        data = &aaData_2;
        currentChartDisplayChannel = 0;
    }
    data->clear();
    data->setDev(round(dev_1*1000)/1000);
    data->setWCCPeakZ(round(point_0.z*1000));
    data->setXTilt(round(xTilt_1*10000)/10000);
    data->setYTilt(round(yTilt_1*10000)/10000);
    data->setZPeak(map["zPeak"].toDouble()*1000);
    //Layer 0:
    data->setLayer0(QString("L0 -- CC:")
                    .append(QString::number(point_0.z, 'g', 6))
    );
    //Layer 1:
    if (points_1.size() > 0) {
        data->setLayer1(QString("L1- XT:")
                           .append(QString::number(xTilt_1,'g',3))
                           .append(" YT:")
                           .append(QString::number(yTilt_1,'g',3))
                           .append(" UL:")
                           .append(QString::number(points_1[0].z,'g',6))
                           .append(" UR:")
                           .append(QString::number(points_1[3].z,'g',6))
                           .append(" LL:")
                           .append(QString::number(points_1[1].z,'g',6))
                           .append(" LR:")
                           .append(QString::number(points_1[2].z,'g',6))
                           .append(" DEV:")
                           .append(QString::number(dev_1,'g',3))
        );
    }
    //Layer 2:
    if (points_2.size() > 0) {
        data->setLayer2(QString("L2- XT:")
                           .append(QString::number(xTilt_2,'g',3))
                           .append(" YT:")
                           .append(QString::number(yTilt_2,'g',3))
                           .append(" UL:")
                           .append(QString::number(points_2[0].z,'g',6))
                           .append(" UR:")
                           .append(QString::number(points_2[3].z,'g',6))
                           .append(" LL:")
                           .append(QString::number(points_2[1].z,'g',6))
                           .append(" LR:")
                           .append(QString::number(points_2[2].z,'g',6))
                           .append(" DEV:")
                           .append(QString::number(dev_2,'g',3))
        );
    }
    //Layer 3
    if (points_3.size() > 0) {
        data->setLayer3(QString("L3- XT:")
                           .append(QString::number(xTilt_3,'g',3))
                           .append(" YT:")
                           .append(QString::number(yTilt_3,'g',3))
                           .append(" UL:")
                           .append(QString::number(points_3[0].z,'g',6))
                           .append(" UR:")
                           .append(QString::number(points_3[3].z,'g',6))
                           .append(" LL:")
                           .append(QString::number(points_3[1].z,'g',6))
                           .append(" LR:")
                           .append(QString::number(points_3[2].z,'g',6))
                           .append(" DEV:")
                           .append(QString::number(dev_3,'g',3))
        );
    }

    int display_layer = validLayer-1;

    if ( display_layer > aaCoreConfig->AASelectedLayer() )
    {
        display_layer = aaCoreConfig->AASelectedLayer();
    }

    qCInfo(logCate()) << "display layer: " << display_layer;
    if (display_layer == 0) {
        result.insert("xTilt", xTilt_1);
        result.insert("yTilt", yTilt_1);
    } else if (display_layer == 1) {
        result.insert("xTilt", xTilt_2);
        result.insert("yTilt", yTilt_2);
    } else if (display_layer == 2) {
        result.insert("xTilt", xTilt_3);
        result.insert("yTilt", yTilt_3);
    }

    QVariantMap sfrMap;
    for(size_t i = 0; i < sorted_sfr_map[0].size(); i++)
    {
        data->addData(0, sorted_sfr_map[0][i].pz*1000, sorted_sfr_fit_map[0][i], sorted_sfr_map[0][i].sfr);


        if (points_1.size() > 0) {
            data->addData(1, sorted_sfr_map[1+4*display_layer][i].pz*1000, sorted_sfr_fit_map[1+4*display_layer][i], sorted_sfr_map[1+4*display_layer][i].sfr);
            data->addData(2, sorted_sfr_map[4+4*display_layer][i].pz*1000, sorted_sfr_fit_map[4+4*display_layer][i], sorted_sfr_map[4+4*display_layer][i].sfr);
            data->addData(3, sorted_sfr_map[3+4*display_layer][i].pz*1000, sorted_sfr_fit_map[3+4*display_layer][i], sorted_sfr_map[3+4*display_layer][i].sfr);
            data->addData(4, sorted_sfr_map[2+4*display_layer][i].pz*1000, sorted_sfr_fit_map[2+4*display_layer][i], sorted_sfr_map[2+4*display_layer][i].sfr);
        }
    }
    data->plot();

    {
        QVariantMap sfrMap;
        for(size_t i = 0; i < sorted_sfr_map[0].size(); i++)
        {
            QVariantMap s;
            s.insert("index", i);
            s.insert("px", sorted_sfr_map[0][i].px);
            s.insert("py", sorted_sfr_map[0][i].py);
            s.insert("area", sorted_sfr_map[0][i].area);
            s.insert("sfr", sorted_sfr_map[0][i].sfr);
            s.insert("t_sfr", sorted_sfr_map[0][i].t_sfr);
            s.insert("b_sfr", sorted_sfr_map[0][i].b_sfr);
            s.insert("l_sfr", sorted_sfr_map[0][i].l_sfr);
            s.insert("r_sfr", sorted_sfr_map[0][i].r_sfr);
            s.insert("pz", sorted_sfr_map[0][i].pz);
            sfrMap.insert(QString::number(i), s);
        }
        QString indexString = "CC";
        map.insert(indexString, sfrMap);
    }
    if (validLayer>=1) {
        for (size_t j = 0; j < 4; j++){
            QVariantMap sfrMap;
            for(size_t i = 0; i < sorted_sfr_map[1].size(); i++)
            {
                QVariantMap s;
                s.insert("index", i);
                s.insert("px", sorted_sfr_map[1+j][i].px);
                s.insert("py", sorted_sfr_map[1+j][i].py);
                s.insert("area", sorted_sfr_map[1+j][i].area);
                s.insert("sfr", sorted_sfr_map[1+j][i].sfr);
                //s.insert("dfov", current_dfov[QString::number(i)]);
                s.insert("t_sfr", sorted_sfr_map[1+j][i].t_sfr);
                s.insert("b_sfr", sorted_sfr_map[1+j][i].b_sfr);
                s.insert("l_sfr", sorted_sfr_map[1+j][i].l_sfr);
                s.insert("r_sfr", sorted_sfr_map[1+j][i].r_sfr);
                s.insert("pz", sorted_sfr_map[1+j][i].pz);
                sfrMap.insert(QString::number(i), s);
            }
            QString indexString = "";
            if (j==0) indexString = "UL_1";
            if (j==1) indexString = "LL_1";
            if (j==2) indexString = "LR_1";
            if (j==3) indexString = "UR_1";
            map.insert(indexString, sfrMap);
        }
        map.insert("xPeak_1_UL", points_1[0].x);
        map.insert("xPeak_1_LL", points_1[1].x);
        map.insert("xPeak_1_LR", points_1[2].x);
        map.insert("xPeak_1_UR", points_1[3].x);
        map.insert("yPeak_1_UL", points_1[0].y);
        map.insert("yPeak_1_LL", points_1[1].y);
        map.insert("yPeak_1_LR", points_1[2].y);
        map.insert("yPeak_1_UR", points_1[3].y);
        map.insert("zPeak_1_UL", points_1[0].z);
        map.insert("zPeak_1_LL", points_1[1].z);
        map.insert("zPeak_1_LR", points_1[2].z);
        map.insert("zPeak_1_UR", points_1[3].z);
    }
    if (validLayer>=2) {
        for (size_t j = 0; j < 4; j++){
            QVariantMap sfrMap;
            for(size_t i = 0; i < sorted_sfr_map[5].size(); i++)
            {
                QVariantMap s;
                s.insert("index", i);
                s.insert("px", sorted_sfr_map[5+j][i].px);
                s.insert("py", sorted_sfr_map[5+j][i].py);
                s.insert("area", sorted_sfr_map[5+j][i].area);
                s.insert("sfr", sorted_sfr_map[5+j][i].sfr);
                //s.insert("dfov", current_dfov[QString::number(i)]);
                s.insert("t_sfr", sorted_sfr_map[5+j][i].t_sfr);
                s.insert("b_sfr", sorted_sfr_map[5+j][i].b_sfr);
                s.insert("l_sfr", sorted_sfr_map[5+j][i].l_sfr);
                s.insert("r_sfr", sorted_sfr_map[5+j][i].r_sfr);
                s.insert("pz", sorted_sfr_map[5+j][i].pz);
                sfrMap.insert(QString::number(i), s);
            }
            QString indexString = "";
            if (j==0) indexString = "UL_2";
            if (j==1) indexString = "LL_2";
            if (j==2) indexString = "LR_2";
            if (j==3) indexString = "UR_2";
            map.insert(indexString, sfrMap);
        }
        map.insert("xPeak_2_UL", points_2[0].x);
        map.insert("xPeak_2_LL", points_2[1].x);
        map.insert("xPeak_2_LR", points_2[2].x);
        map.insert("xPeak_2_UR", points_2[3].x);
        map.insert("yPeak_2_UL", points_2[0].y);
        map.insert("yPeak_2_LL", points_2[1].y);
        map.insert("yPeak_2_LR", points_2[2].y);
        map.insert("yPeak_2_UR", points_2[3].y);
        map.insert("zPeak_2_UL", points_2[0].z);
        map.insert("zPeak_2_LL", points_2[1].z);
        map.insert("zPeak_2_LR", points_2[2].z);
        map.insert("zPeak_2_UR", points_2[3].z);
    }
    if (validLayer>=3) {
        for (size_t j = 0; j < 4; j++){
            QVariantMap sfrMap;
            for(size_t i = 0; i < sorted_sfr_map[1].size(); i++)
            {
                QVariantMap s;
                s.insert("index", i);
                s.insert("px", sorted_sfr_map[9+j][i].px);
                s.insert("py", sorted_sfr_map[9+j][i].py);
                s.insert("area", sorted_sfr_map[9+j][i].area);
                s.insert("sfr", sorted_sfr_map[9+j][i].sfr);
                //s.insert("dfov", current_dfov[QString::number(i)]);
                s.insert("t_sfr", sorted_sfr_map[9+j][i].t_sfr);
                s.insert("b_sfr", sorted_sfr_map[9+j][i].b_sfr);
                s.insert("l_sfr", sorted_sfr_map[9+j][i].l_sfr);
                s.insert("r_sfr", sorted_sfr_map[9+j][i].r_sfr);
                s.insert("pz", sorted_sfr_map[9+j][i].pz);
                sfrMap.insert(QString::number(i), s);
            }
            QString indexString = "";
            if (j==0) indexString = "UL_3";
            if (j==1) indexString = "LL_3";
            if (j==2) indexString = "LR_3";
            if (j==3) indexString = "UR_3";
            map.insert(indexString, sfrMap);
        }
        map.insert("xPeak_3_UL", points_3[0].x);
        map.insert("xPeak_3_LL", points_3[1].x);
        map.insert("xPeak_3_LR", points_3[2].x);
        map.insert("xPeak_3_UR", points_3[3].x);
        map.insert("yPeak_3_UL", points_3[0].y);
        map.insert("yPeak_3_LL", points_3[1].y);
        map.insert("yPeak_3_LR", points_3[2].y);
        map.insert("yPeak_3_UR", points_3[3].y);
        map.insert("zPeak_3_UL", points_3[0].z);
        map.insert("zPeak_3_LL", points_3[1].z);
        map.insert("zPeak_3_LR", points_3[2].z);
        map.insert("zPeak_3_UR", points_3[3].z);
        //map.insert("fov_slope", current_fov_slope);
    }

//    emit postSfrDataToELK(runningUnit, map);
    return result;
}

double dot_product(threeDPoint a, threeDPoint b) {
    return a.x * b.x + a.y * b.y + a.z * b.z;
}

void normalize(threeDPoint & vec) {
    double len = vec.x * vec.x + vec.y * vec.y + vec.z * vec.z;
    len = sqrt(len);
    if (len <= 0)
        return;
    vec.x /= len;
    vec.y /= len;
    vec.z /= len;
}

void get_plane(const threeDPoint & centroid, const threeDPoint & normal, double & a, double & b, double & c, double & d) {
    d = -(centroid.x * normal.x + centroid.y * normal.y + centroid.z * normal.z);
    a = normal.x;
    b = normal.y;
    c = normal.z;
}

threeDPoint AACore::aaCorePlaneFitting(std::vector<threeDPoint> points)
{
    double xx_sum = 0, yy_sum = 0, zz_sum = 0;
    int total = (int)points.size();
    for (int i = 0; i < total; ++i) {
        xx_sum += points[i].x;
        yy_sum += points[i].y;
        zz_sum += points[i].z;
    }
    double cx = xx_sum / total;
    double cy = yy_sum / total;
    double cz = zz_sum / total;
    threeDPoint centroid(cx, cy, cz);
    double xx = 0, xy = 0, xz = 0;
    double yy = 0, yz = 0, zz = 0;
    for (int i = 0; i < total; ++i) {
        double dev_x = points[i].x - cx;
        double dev_y = points[i].y - cy;
        double dev_z = points[i].z - cz;
        xx += dev_x * dev_x;
        xy += dev_x * dev_y;
        xz += dev_x * dev_z;
        yy += dev_y * dev_y;
        yz += dev_y * dev_z;
        zz += dev_z * dev_z;
    }
    xx /= total;
    xy /= total;
    xz /= total;
    yy /= total;
    yz /= total;
    zz /= total;
    // Method 1
    threeDPoint weighted_dir = threeDPoint(0, 0, 0);
    {
        double det_x = yy * zz - yz * yz;
        double axis_x = det_x;
        double axis_y = xz * yz - xy * zz;
        double axis_z = xy * yz - xz * yy;
        threeDPoint axis_dir(axis_x, axis_y, axis_z);
        //std::cout << "axis_dir: (" << axis_dir.x << ", " << axis_dir.y << ", " << axis_dir.z << ")" << std::endl;
        double weight = det_x * det_x;
        if (dot_product(weighted_dir, axis_dir) < 0.0) {
            weight = -weight;
        }
        weighted_dir.x += axis_dir.x * weight;
        weighted_dir.y += axis_dir.y * weight;
        weighted_dir.z += axis_dir.z * weight;
    }
    {
        double det_y = xx * zz - xz * xz;
        double axis_x = xz * yz - xy * zz;
        double axis_y = det_y;
        double axis_z = xy * xz - yz * xx;
        threeDPoint axis_dir(axis_x, axis_y, axis_z);
        //std::cout << "axis_dir: (" << axis_dir.x << ", " << axis_dir.y << ", " << axis_dir.z << ")" << std::endl;
        double weight = det_y * det_y;
        if (dot_product(weighted_dir, axis_dir) < 0.0) {
            weight = -weight;
        }
        weighted_dir.x += axis_dir.x * weight;
        weighted_dir.y += axis_dir.y * weight;
        weighted_dir.z += axis_dir.z * weight;
    }
    {
        double det_z = xx * yy - xy * xy;
        double axis_x = xy * yz - xz * yy;
        double axis_y = xy * xz - yz * xx;
        double axis_z = det_z;
        threeDPoint axis_dir(axis_x, axis_y, axis_z);
        //std::cout << "axis_dir: (" << axis_dir.x << ", " << axis_dir.y << ", " << axis_dir.z << ")" << std::endl;
        double weight = det_z * det_z;
        if (dot_product(weighted_dir, axis_dir) < 0.0) {
            weight = -weight;
        }
        weighted_dir.x += axis_dir.x * weight;
        weighted_dir.y += axis_dir.y * weight;
        weighted_dir.z += axis_dir.z * weight;
        normalize(weighted_dir);
        double a, b, c, d;
        get_plane(centroid, weighted_dir, a, b, c, d);
        //double error = cal_error(points, a, b, c, d);
        //Convert to degree
        weighted_dir.x *= 180.0/M_PI;
        weighted_dir.y *= 180.0/M_PI;

        return weighted_dir;
    }
}

double AACore::calculateDFOV(cv::Mat input_img)
{
    pattern_v.clear();
    AA_Helper::AAA_Search_MTF_Pattern_Ex_New(input_img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea(), 1, pattern_v);

    if (pattern_v.size() == 0) {
        qCWarning(logCate()) << tr("Cannot find any mtf pattern");
        return -1;
    }
    if (pattern_v.size() == 4) {
        double d1 = sqrt(pow((pattern_v[0].center.x() - pattern_v[2].center.x()), 2) + pow((pattern_v[0].center.y() - pattern_v[2].center.y()), 2));
        double d2 = sqrt(pow((pattern_v[3].center.x() - pattern_v[1].center.x()), 2) + pow((pattern_v[3].center.y() - pattern_v[1].center.y()), 2));
        double f = aaCoreConfig->EFL();
        double dfov1 = 2*atan(d1/(2*aaCoreConfig->SensorXRatio()*f))*180/PI;
        double dfov2 = 2*atan(d2/(2*aaCoreConfig->SensorYRatio()*f))*180/PI;
        double dfov = (dfov1 + dfov2)/2;
        return dfov;
    }
    return -1;
}

bool AACore::performAAOffline(QJsonValue params)
{
    SCTimer t("aaOffline", aaCoreTimeLog());
    clustered_sfr_map.clear();
    QVariantMap map, stepTimerMap, dFovMap, sfrTimeElapsedMap;
    int resize_factor = 1;
    int sfrCount = 0;
    double step_size = 0.01, start = 20;
    double xsum=0,x2sum=0,ysum=0,xysum=0;
    double estimated_fov_slope = 15;

    int inputImageCount = 7;

    for (int i = 0; i < inputImageCount; i++)
    {
        //QString filename = "C:\\Users\\emil\\Desktop\\AAProject\\newSingleHead\\2020-09-07\\zscan_" + QString::number(i) + ".jpg";

        QString filename = "C:\\Users\\emil\\Desktop\\AAProject\\newSingleHead\\Z SCAN 2020-09-08\\zscan_" + QString::number(i) + ".jpg";

        //QString filename = "C:\\Users\\emil\\Desktop\\sunny_field\\aascan\\2\\2\\zscan_" + QString::number(i+1) + ".bmp";
        cv::Mat input_img = cv::imread(filename.toStdString());

        double dfov = calculateDFOV(input_img);
        qCInfo(logCate()) << "DFOV : " << dfov;

        if (input_img.empty()) {
            qCWarning(logCate()) << tr("Perform AA fail due to cannot grab image");
            return false;
        }

        resize_factor = aaCoreConfig->Oversampling() + 1;
        cv::Mat img;
        cv::Size size(input_img.cols/resize_factor, input_img.rows/resize_factor);
        cv::resize(input_img, img, size);

        emit sfrWorkerController->calculate(i, start+i*step_size, img, aaCoreConfig->MaxIntensity(), aaCoreConfig->MinArea(), aaCoreConfig->MaxArea(), aaCoreConfig->SampleFrequency()+1, aaCoreConfig->AASelectedLayer()+1);
        img.release();
        sfrCount++;
    }

    int timeout=2000;
    while(this->clustered_sfr_map.size() != sfrCount && timeout > 0) {
        Sleep(10);
        timeout--;
    }
    if (timeout <= 0) {
        qCCritical(logCate())<< tr("Error in performing AA Offline: %1").arg(timeout);
        return false;
    }
    QVariantMap aa_result = sfrFitCurve_Advance(resize_factor, start);
    double xTilt = aa_result["xTilt"].toDouble();
    double yTilt = aa_result["yTilt"].toDouble();
    qCInfo(logCate()) << "xTilt: " << xTilt << " yTilt: " << yTilt;
    clustered_sfr_map.clear();

    return true;
}

bool AACore::performDispense(QJsonValue params)
{
    SCTimer t(QString("Dispense"), aaCoreTimeLog());
    double x_offset_in_um = params["x_offset_in_um"].toDouble(0);
    double y_offset_in_um = params["y_offset_in_um"].toDouble(0);
    double z_offset_in_um = params["z_offset_in_um"].toDouble(0);
    bool enable_glue_inspection = params["enable_glue_inspection"].toInt(0);
    int glue_inspection_mode = params["glue_inspection_mode"].toInt(0);
    double max_glue_width_in_mm = params["max_glue_width_in_mm"].toDouble(0);
    double min_glue_width_in_mm = params["min_glue_width_in_mm"].toDouble(0);
    double max_avg_glue_width_in_mm = params["max_avg_glue_width_in_mm"].toDouble(0);
    qDebug("enable_glue_inspection: %d  mode: %d %f %f %f", enable_glue_inspection, glue_inspection_mode,
           max_glue_width_in_mm, min_glue_width_in_mm, max_avg_glue_width_in_mm);
    int finish_delay = params["delay_in_ms"].toInt(0);

    if (enable_glue_inspection == 1) {
        this->aaHead->moveToShotGlue(true);
    } else {
        this->aaHead->moveToShotGlue(false);
    }
    setGoodProduct();
    return true;
}

bool AACore::performZOffset(QJsonValue params)
{
    SCTimer t(QString("Z Offset"), aaCoreTimeLog());
    double z_offset_in_um = params["z_offset_in_um"].toDouble(0)/1000;
    this->sUTXYZ->zAxis()->relMove(z_offset_in_um);
    return true;
}

bool AACore::performUV(QJsonValue params)
{
    SCTimer t(QString("UV"), aaCoreTimeLog());
    if (aaHead == Q_NULLPTR || aaHead->getUVIO() == Q_NULLPTR) {
        qCWarning(logCate()) << "AA Head config fail. Perform UV Fail";
        return false;
    }
    aaHead->doUV();
    return true;
}
